#include "i2c.h"
#include "platform_i2c.h"
#include "semaphores.h"
#include "sh_mem.h"
#include "os_io.h"

static bool i2c_api_inited = false;
static i2c_access_mem_t *i2c_mem = NULL;

#define CHECK_I2C_API_INIT()  \
    if (!i2c_api_inited) {     \
        err_code = i2c_api_init();  \
	if (err_code != ERR_SUCCESS) {  \
	    printf("Failed ot init i2c api\n");  \
	    return ERR_INTERNAL_ERROR;  \
	}  \
    }


static err_t i2c_api_init(void)
{
    err_t err_code = ERR_SUCCESS;
    void *handle = NULL;
    bool created = false;
    void *addr;

    err_code = create_shared_memory((uint8_t *)"i2c_access_mem", SHM_CREATE_IF_NOT_FOUND,
                sizeof(i2c_access_mem_t), &handle, &addr, &created);
    if (err_code != ERR_SUCCESS) {
        return err_code;
    }

    i2c_mem = (i2c_access_mem_t *)addr;

    if (created) {
        // Memory created for the first time, Initialize it.
	err_code = sem_create2(&i2c_mem->i2c_lock, true, false);
	if (err_code != ERR_SUCCESS) {
	    return err_code;
	}
    }

    i2c_api_inited = true;
    
    return ERR_SUCCESS;
}



err_t read_dip_switch(uint8_t switch_no, bool *switch_state)
{
    err_t err_code = 0;
    uint8_t value = 0;
    
    CHECK_I2C_API_INIT();  // macro can cause return!!

    err_code = sem_lock(&i2c_mem->i2c_lock);
    if (err_code != ERR_SUCCESS) {
        return err_code;
    }

    i2c_read_reg(I2C0, I2CE_INPUT_PORT1, &value);
    if (value & (0x1 << switch_no)) {
        // Switch is off
	*switch_state = false;
    } else {
        *switch_state = true;
    }

    err_code = sem_unlock(&i2c_mem->i2c_lock);

    return err_code;

}


err_t glow_led(module_led_t led, bool led_state)
{
    err_t err_code = 0;
    uint8_t value = 0;
    
    CHECK_I2C_API_INIT();  // macro can cause return!!

    //printf("glowing led to 0x%x\n", led_state);
    err_code = sem_lock(&i2c_mem->i2c_lock);
    if (err_code != ERR_SUCCESS) {
        printf("Error in locking semaphore\n");
        return err_code;
    }

    i2c_read_reg(I2C0, I2CE_INPUT_PORT0, &value);
    //printf("Read value 0x%x from led port\n", value);

    if (led == LED1) {
        if (led_state) {
	    value = value & 0x7F;
	} else {
	    value = value | 0x80;
	}
    } else {
        // LED2
	if (led_state) {
	    value = value & 0xBF;
	} else {
	    value = value | 0x40;
	}
    }

    //printf("Writing value to led: 0x%x\n", value);
    i2c_write_reg(I2C0, I2CE_OUTPUT_PORT0, value);

    err_code = sem_unlock(&i2c_mem->i2c_lock);

    //printf("Exiting with semaphore value: 0x%x\n", err_code);
    return err_code;

}
